#include "bsp_pid_controller.h"
#include <string.h>
#include <math.h>
#include "bsp_def.h"

/**
 * @brief 初始化PID控制器
 * @param pid: PID控制器指针
 * @param mode: PID模式 (位置式/增量式)
 * @param dt: 控制周期(秒) - 调试时需要根据实际控制周期修改
 */
void pid_init(pid_controller_t *pid, pid_mode_t mode, float dt)
{
    memset(pid, 0, sizeof(pid_controller_t));
    
    // ========== 默认参数 - 调试时主要修改这里 ==========
    pid->kp = DEFAULT_PID_KP;
    pid->ki = DEFAULT_PID_KI;
    pid->kd = DEFAULT_PID_KD;
    
    // ========== 默认限幅 - 调试时根据实际情况调整 ==========
    pid->output_limit = DEFAULT_PID_MAX_OUTPUT;
    pid->integral_limit = DEFAULT_PID_MAX_INTEGRAL;
    
    // ========== 配置参数 ==========
    pid->mode = mode;
    pid->dt = dt;           // 重要：根据实际控制周期设置
    pid->enable = true;
    
    // 初始化所有状态变量
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->prev_error2 = 0.0f;
    pid->prev_output = 0.0f;
    pid->prev_measurement = 0.0f;
    
    // 初始化调试信息
    pid->last_output = 0.0f;
    pid->last_error = 0.0f;
    pid->last_p_term = 0.0f;
    pid->last_i_term = 0.0f;
    pid->last_d_term = 0.0f;
    pid->update_count = 0;
}

/**
 * @brief 设置PID参数 - 调试时主要调用这个函数
 * @param kp: 比例系数 - 增大使响应更快，但过大可能振荡
 * @param ki: 积分系数 - 增大消除静差，但过大可能超调
 * @param kd: 微分系数 - 增大抑制超调，但过大可能对噪声敏感
 */
void pid_set_parameters(pid_controller_t *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}

/**
 * @brief 设置限幅参数 - 调试时根据电机能力调整
 */
void pid_set_limits(pid_controller_t *pid, float output_limit, float integral_limit)
{
    pid->output_limit = output_limit;
    pid->integral_limit = integral_limit;
}

/**
 * @brief 重置PID控制器
 */
void pid_reset(pid_controller_t *pid)
{
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->prev_error2 = 0.0f;
    pid->prev_output = 0.0f;
    pid->prev_measurement = 0.0f;
    
    pid->last_output = 0.0f;
    pid->last_error = 0.0f;
    pid->last_p_term = 0.0f;
    pid->last_i_term = 0.0f;
    pid->last_d_term = 0.0f;
    pid->update_count = 0;
}

/**
 * @brief PID计算（自动选择模式）
 */
float pid_calculate(pid_controller_t *pid, float target, float measurement)
{
    if (!pid->enable) {
        return 0.0f;
    }
    
    switch (pid->mode) {
        case PID_MODE_POSITION:
            return pid_calculate_position(pid, target, measurement);
        case PID_MODE_VELOCITY:
            return pid_calculate_velocity(pid, target, measurement);
        default:
            return 0.0f;
    }
}

/**
 * @brief 位置式PID计算 - 适合位置控制
 * 公式: Output = Kp*e + Ki*∫e*dt + Kd*de/dt
 * 微分项对测量值微分，减少设定值变化的冲击
 */
float pid_calculate_position(pid_controller_t *pid, float target, float measurement)
{
    float error = target - measurement;
    
    // 比例项
    float p_term = pid->kp * error;
    
    // 积分项（带限幅）
    pid->integral += error * pid->dt;
    
    // ========== 积分限幅 - 调试时可调整限幅值 ==========
    if (pid->integral > pid->integral_limit) {
        pid->integral = pid->integral_limit;
    } else if (pid->integral < -pid->integral_limit) {
        pid->integral = -pid->integral_limit;
    }
    
    float i_term = pid->ki * pid->integral;
    
    // 微分项（对测量值微分，减少setpoint变化的冲击）
    float derivative = (measurement - pid->prev_measurement) / pid->dt;
    float d_term = -pid->kd * derivative;  // 注意负号
    
    // 计算总输出
    float output = p_term + i_term + d_term;
    
    // ========== 输出限幅 - 调试时根据电机最大电流调整 ==========
    if (output > pid->output_limit) {
        output = pid->output_limit;
    } else if (output < -pid->output_limit) {
        output = -pid->output_limit;
    }
    
    // 保存各项分量用于调试
    pid->last_p_term = p_term;
    pid->last_i_term = i_term;
    pid->last_d_term = d_term;
    
    // 更新状态
    pid->prev_error = error;
    pid->prev_measurement = measurement;
    pid->last_output = output;
    pid->last_error = error;
    pid->update_count++;
    
    return output;
}

/**
 * @brief 增量式PID计算 - 适合速度控制
 * 公式: ΔOutput = Kp*(e(k)-e(k-1)) + Ki*e(k)*dt + Kd*(e(k)-2e(k-1)+e(k-2))/dt
 * @param pid PID控制器结构体指针
 * @param target 目标值（设定值）
 * @param measurement 测量值（反馈值）
 * @return float 本次控制输出值（绝对量）
 */
float pid_calculate_velocity(pid_controller_t *pid, float target, float measurement)
{
    float error = target - measurement;
    
    // 比例项：当前误差与上次误差的差值
    float p_term = pid->kp * (error - pid->prev_error);
    
    // 积分项：当前误差的积分
    float i_term = pid->ki * error * pid->dt;
    
    // 微分项：误差的二次差分
    float d_term = pid->kd * (error - 2 * pid->prev_error + pid->prev_error2) / pid->dt;
    
    // 计算输出增量
    float delta_output = p_term + i_term + d_term;
    float output = pid->prev_output + delta_output;
    
    // 输出限幅
    if (output > pid->output_limit) {
        output = pid->output_limit;
    } else if (output < -pid->output_limit) {
        output = -pid->output_limit;
    }
    
    // 保存各项分量用于调试
    pid->last_p_term = p_term;
    pid->last_i_term = i_term;
    pid->last_d_term = d_term;
    
    // 更新状态（注意：增量式PID不需要积分项累积）
    pid->prev_error2 = pid->prev_error;  // 上上次误差
    pid->prev_error = error;             // 上次误差
    pid->prev_output = output;           // 上次输出
    pid->last_output = output;
    pid->last_error = error;
    pid->update_count++;

    return output;
}

/**
 * @brief 获取最后输出值（调试用）
 */
float pid_get_last_output(pid_controller_t *pid)
{
    return pid->last_output;
}

/**
 * @brief 获取最后误差值（调试用）
 */
float pid_get_last_error(pid_controller_t *pid)
{
    return pid->last_error;
}

/**
 * @brief 获取PID各项分量（调试用）
 */
void pid_get_debug_info(pid_controller_t *pid, float *p_term, float *i_term, float *d_term)
{
    if (p_term) *p_term = pid->last_p_term;
    if (i_term) *i_term = pid->last_i_term;
    if (d_term) *d_term = pid->last_d_term;
}